/*************************************************************************
    Projet  -  Sport Collectif pour Robots
                             -------------------
    Auteur  -  Kevin Vandel
*************************************************************************/

//-- Realisation de la classe <Communication> (fichier Communication.cpp)

//---------------------------------------------------------------- INCLUDE

//-------------------------------------------------------- Include systeme
#include <iostream>
#include <sstream>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include "sys/time.h"

//------------------------------------------------------ Include personnel
#include "Communication.h"
#include "../config.h"
#include "../util.h"

//------------------------------------------------------------- Constantes

#define RECEPTION_SLEEP_TIME 1000 //Attente entre deux lecture (en microsecondes)

// Taille minimale d'un message envoye par le Khepera :
#define MIN_MSG_SIZE 3 // 1 caractere + 2 caracteres fin de message

// Caracteres de fin de chaine envoye par le Khepera pour identifier la fin d'un message :
#define END_RCV_COMMAND_1 13 //'\r'
#define END_RCV_COMMAND_2 10 //'\c'

// Caracteres speciaux pour la communication avec le Khepera
#define END_SEND_COMMAND					'\n'
#define KH_CMD_SEPARATOR					','
#define KH_CMD_CONFIGURE_PID_SPEED			'A'
#define KH_RETURN_CMD_CONFIGURE_PID_SPEED	'a'
#define KH_CMD_GET_VERSION					'B'
#define KH_RETURN_CMD_GET_VERSION			'b'
#define KH_CMD_SET_POSITION					'C'
#define KH_RETURN_CMD_SET_POSITION			'c'
#define KH_CMD_SET_SPEED					'D'
#define KH_RETURN_CMD_SET_SPEED				'd'
#define KH_CMD_GET_SPEED					'E'
#define KH_RETURN_CMD_GET_SPEED				'e'
#define KH_CMD_CONFIGURE_PID_POS			'F'
#define KH_RETURN_CMD_CONFIGURE_PID_POS		'f'
#define KH_CMD_SET_POSITION_COUNTER			'G'
#define KH_RETURN_CMD_SET_POSITION_COUNTER	'g'
#define KH_CMD_GET_POSITION					'H'
#define KH_RETURN_CMD_GET_POSITION			'h'
#define KH_CMD_CONF_POS_CONTROLLER			'J'
#define KH_RETURN_CMD_CONF_POS_CONTROLLER	'j'


//----------------------------------------------------------------- PUBLIC

//----------------------------------------------------- Methodes publiques
void Communication::Fermer()
{
	if( port != "" ) //Si un port est ouvert
	{
		LOG4CPLUS_TRACE(loggerCom, "[COM] Fermeture du port : " << port);
		close(fileDescriptor);
		port = "";
	}
	fileDescriptor = -1;
} //----- Fin de Fermer


bool Communication::Ouvrir(string openPort)
{
	bool cr = true;
	if( port != "" ) //Si un port est ouvert
	{
		errno = ER_PORT_COM_ALREADY_OPENED;
		cr = false;
	}

	// Ouverture du port <openPort> */
	if( (fileDescriptor = open(openPort.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK)) < 0 )
	{
		errno = ER_PORT_COM_NOT_OPENED;
		cr = false;
	}
	port = openPort;
	LOG4CPLUS_TRACE(loggerCom, "[COM] Ouverture du port : " << port);

	return cr;
} //----- Fin de Ouvrir


string const& Communication::getOpenedPort() const
{
	return port;
} //----- Fin de getOpenedPort


bool Communication::ConfigurerPort(BaudRate baudRate, DataBits countBits, Parity parity, StopBits countBitStop)
{
	bool cr = true;
	if( port == "" ) //Si aucun port ouvert
	{
		errno = ER_PORT_COM_NOT_OPENED;
		cr = false;
	}
	else
	{
		/* Initialisation du port COM */
		// Modes locaux
		configuration.c_lflag &= ~ECHO;
		// Modes d'entree
		configuration.c_iflag = IGNBRK | IGNPAR; // | IXOFF;
		// Modes de sortie
		configuration.c_oflag = 0;
		// Modes de controle
		configuration.c_cflag = CREAD | CLOCAL;
		configuration.c_cflag = configuration.c_cflag & ~CRTSCTS;
		// Vitesse
		switch(baudRate)
		{
			case Communication::CCOM_BR600  :	configuration.c_cflag |= B600; break;
			case Communication::CCOM_BR1200 :	configuration.c_cflag |= B1200; break;
			case Communication::CCOM_BR2400 :	configuration.c_cflag |= B2400; break;
			case Communication::CCOM_BR4800 :	configuration.c_cflag |= B4800; break;
			case Communication::CCOM_BR9600 :	configuration.c_cflag |= B9600; break;
			case Communication::CCOM_BR19200:	configuration.c_cflag |= B19200; break;
			case Communication::CCOM_BR57600:	configuration.c_cflag |= B57600; break;
			default:
				errno = Communication::ER_BAUD_RATE_UNDEFINED;
				cr = false;
				break;
		}
		// Longueur des caracteres
		switch(countBits)
		{
			case Communication::CCOM_5_BITS :	configuration.c_cflag |= CS5; break;
			case Communication::CCOM_6_BITS :	configuration.c_cflag |= CS6; break;
			case Communication::CCOM_7_BITS :	configuration.c_cflag |= CS7; break;
			case Communication::CCOM_8_BITS :	configuration.c_cflag |= CS8; break;
			default:
				errno = Communication::ER_CAR_SIZE_UNDEFINED;
				cr = false;
				break;
		}
		// Parite
		switch(parity)
		{
			case Communication::CCOM_NONE : /* rien a faire */ break;
			case Communication::CCOM_ODD  : configuration.c_cflag |= PARODD; break;
			case Communication::CCOM_EVEN : configuration.c_cflag |= PARENB; break;
			default:
				errno = Communication::ER_PARITY_MODE_UNDEFINED;
				cr = false;
				break;
		}
		// Nombre de bits de stop
		switch(countBitStop)
		{
			case Communication::CCOM_1_BIT  : /* rien a faire */ break;
			case Communication::CCOM_2_BIT : configuration.c_cflag |= CSTOPB; break;
			default:
				errno = Communication::ER_COUNT_STOP_BIT_UNDEFINED;
				cr = false;
				break;
		}

		// Envoi de la configuration
		if( tcsetattr(fileDescriptor, TCSANOW, &configuration) == -1 )
		{
			errno = Communication::ER_CONFIGURATION_UNAPPLICABLE;
			cr = false;
		}
		tcflush(fileDescriptor, TCIOFLUSH);	//On vide le buffer d'E/S
	}
	return cr;
} //----- Fin de ConfigurerPort


/************************** COMMANDES KHEPERA ***************************/
// Pour plus de details sur les commandes, se referer a l'annexe du manuel.

bool Communication::ConfigurerPidVitesse(int Kp, int Ki, int Kd)
{
	ostringstream os;
	os << KH_CMD_CONFIGURE_PID_SPEED << KH_CMD_SEPARATOR << Kp <<
			KH_CMD_SEPARATOR << Ki << KH_CMD_SEPARATOR << Kd << END_SEND_COMMAND;
	string data = os.str();
	string received_data;

	LOG4CPLUS_DEBUG(loggerCom, "[COM] " << port << " - ConfigurerPidVitesse : " << data);

	envoyerDonnees(data);
	recevoirDonnees(received_data);

	return (received_data[0] == KH_RETURN_CMD_CONFIGURE_PID_SPEED);
} //----- Fin de ConfigurerPidVitesse


pair<string, string> Communication::GetVersionLogiciel()
{
	ostringstream os;
	os << KH_CMD_GET_VERSION << END_SEND_COMMAND;
	string data = os.str();
	string received_data;

	LOG4CPLUS_DEBUG(loggerCom, "[COM] " << port << " - ConfigurerPidVitesse : " << data);

	envoyerDonnees(data);
	recevoirDonnees(received_data);

	// Recuperation des donnes recues :
	pair<string, string> result;
	if(received_data[0]==KH_RETURN_CMD_GET_VERSION)
	{
		istringstream iss(received_data);
		string carReponse;
		getline(iss, carReponse, KH_CMD_SEPARATOR); 	// Normalement "b"
		getline(iss, result.first, KH_CMD_SEPARATOR);	// Version du BIOS
		getline(iss, result.second, KH_CMD_SEPARATOR);	// Version du protocole
	}

	return result;
} //----- Fin de GetVersionLogiciel


bool Communication::SetPositionRoues(int roueGauche, int roueDroite)
{
	ostringstream os;
	os << KH_CMD_SET_POSITION << KH_CMD_SEPARATOR << roueGauche << KH_CMD_SEPARATOR << roueDroite << END_SEND_COMMAND;
	string data = os.str();
	string received_data;

	LOG4CPLUS_DEBUG(loggerCom, "[COM] " << port << " - SetPositionRoues : " << data);

	envoyerDonnees(data);
	recevoirDonnees(received_data);

	return (received_data[0] == KH_RETURN_CMD_SET_POSITION);
} //----- Fin de SetPositionRoues


bool Communication::SetVitesseRoues(int roueGauche, int roueDroite)
{
	ostringstream os;
	os << KH_CMD_SET_SPEED << KH_CMD_SEPARATOR << roueGauche << KH_CMD_SEPARATOR << roueDroite << END_SEND_COMMAND;
	string data = os.str();
	string received_data;

	LOG4CPLUS_DEBUG(loggerCom, "[COM] " << port << " - SetVitesseRoues : " << data);

	envoyerDonnees(data);
	recevoirDonnees(received_data);

	return (received_data[0] == KH_RETURN_CMD_SET_SPEED);
} //----- Fin de SetVitesseRoues


pair<int, int> Communication::GetVitesseRoues()
{
	ostringstream os;
	os << KH_CMD_GET_SPEED << END_SEND_COMMAND;
	string data = os.str();
	string received_data;

	LOG4CPLUS_DEBUG(loggerCom, "[COM] " << port << " - GetVitesseRoues : " << data);

	envoyerDonnees(data);
	recevoirDonnees(received_data);

	// Recuperation des donnes recues :
	pair<int, int> result;
	if(received_data[0]==KH_RETURN_CMD_GET_SPEED)
	{
		istringstream iss(received_data);
		string morceau;
		getline(iss, morceau, KH_CMD_SEPARATOR);
		getline(iss, morceau, KH_CMD_SEPARATOR);
		result.first = atoi(morceau.c_str()); //Vitesse roue gauche
		getline(iss, morceau, KH_CMD_SEPARATOR);
		result.second = atoi(morceau.c_str()); //Vitesse roue droite
	}

	return result;
} //----- Fin de GetVitesseRoues


bool Communication::ConfigurerPidPosition(int Kp, int Ki, int Kd)
{
	ostringstream os;
	os << KH_CMD_CONFIGURE_PID_POS << KH_CMD_SEPARATOR << Kp <<
			KH_CMD_SEPARATOR << Ki << KH_CMD_SEPARATOR << Kd << END_SEND_COMMAND;
	string data = os.str();
	string received_data;

	LOG4CPLUS_DEBUG(loggerCom, "[COM] " << port << " - ConfigurerPidPosition : " << data);

	envoyerDonnees(data);
	recevoirDonnees(received_data);

	return (received_data[0] == KH_RETURN_CMD_CONFIGURE_PID_POS);
} //----- Fin de ConfigurerPidPosition


bool Communication::SetCompteurPositionRoues(int roueGauche, int roueDroite)
{
	ostringstream os;
	os << KH_CMD_SET_POSITION_COUNTER << KH_CMD_SEPARATOR << roueGauche << KH_CMD_SEPARATOR << roueDroite << END_SEND_COMMAND;
	string data = os.str();
	string received_data;

	LOG4CPLUS_DEBUG(loggerCom, "[COM] " << port << " - SetCompteurPositionRoues : " << data);

	envoyerDonnees(data);
	recevoirDonnees(received_data);

	return (received_data[0] == KH_RETURN_CMD_SET_POSITION_COUNTER);
} //----- Fin de SetCompteurPositionRoues


pair<int, int> Communication::GetPositionRoues()
{
	ostringstream os;
	os << KH_CMD_GET_POSITION << END_SEND_COMMAND;
	string data = os.str();
	string received_data;

	LOG4CPLUS_DEBUG(loggerCom, "[COM] " << port << " - GetPositionRoues : " << data);

	envoyerDonnees(data);
	recevoirDonnees(received_data);

	// Recuperation des donnes recues :
	pair<int, int> result;
	if(received_data[0]==KH_RETURN_CMD_GET_POSITION)
	{
		istringstream iss(received_data);
		string morceau;
		getline(iss, morceau, KH_CMD_SEPARATOR);
		getline(iss, morceau, KH_CMD_SEPARATOR);
		result.first = atoi(morceau.c_str()); //Position roue gauche
		getline(iss, morceau, KH_CMD_SEPARATOR);
		result.second = atoi(morceau.c_str()); //Position roue droite
	}

	return result;
} //----- Fin de GetPositionRoues


bool Communication::ConfigurerControleurPosition(int vitMaxG, int accG, int vitMaxD, int accD)
{
	ostringstream os;
	os << KH_CMD_CONF_POS_CONTROLLER << KH_CMD_SEPARATOR << vitMaxG << KH_CMD_SEPARATOR << accG <<
					KH_CMD_SEPARATOR << vitMaxD << KH_CMD_SEPARATOR << accD << END_SEND_COMMAND;
	string data = os.str();
	string received_data;

	LOG4CPLUS_DEBUG(loggerCom, "[COM] " << port << " - ConfigurerControleurPosition : " << data);

	envoyerDonnees(data);
	recevoirDonnees(received_data);

	return (received_data[0] == KH_RETURN_CMD_CONF_POS_CONTROLLER);
} //----- Fin de ConfigurerPidPosition


//-------------------------------------------- Constructeurs - destructeur

Communication::Communication()
{
	LOG4CPLUS_TRACE(loggerCom, "Appel au constructeur de <Communication>");
    port = "";
    fileDescriptor = -1;
} //----- Fin de Communication

Communication::~Communication()
{
	LOG4CPLUS_TRACE(loggerCom, "Appel au destructeur de <Communication>");
	Fermer();
} //----- Fin de ~Communication



//------------------------------------------------------------------ PRIVE

//------------------------------------------------------- Methodes privees

bool Communication::recevoirDonnees(string & data)
{
	data.clear();
	char c_car;		//Caratere lu
	int length = 0;	//Nombre de caracteres lus

	//Gestion du timer
	//timeval time;
	//gettimeofday(&time, NULL);

	long timeStampRef = computeTimeStamp(); //time.tv_sec*1000 + time.tv_usec/1000 + 0.5;

	long timeStamp;

	do
	{
		// Mise a jour du timer :
		//gettimeofday(&time, NULL);
		timeStamp = computeTimeStamp(); //time.tv_sec*1000 + time.tv_usec/1000 + 0.5;

		// Lecture caractere par caractere du buffer d'E/S :
		if(read(fileDescriptor, &c_car, 1) > 0)
		{
			data += c_car;
			length++;
		}
		else
		{
			usleep(RECEPTION_SLEEP_TIME);
		}

		// Si le timer est ecoule, on considere que le reception a echoue
		if( (timeStamp - timeStampRef) > RECEPTION_TIMEOUT )
		{
			tcflush(fileDescriptor, TCIOFLUSH);	//On vide le buffer d'E/S
			return false;
		}
	}
	while( length < MIN_MSG_SIZE || (data[length-2] != END_RCV_COMMAND_1 && data[length-1] != END_RCV_COMMAND_2) );

	LOG4CPLUS_DEBUG(loggerCom, "[COM] Data received "<< port  <<" : " << data);

	return true;
} //----- Fin de recevoirDonnees


int Communication::envoyerDonnees(string & data)
{
	//On vide le buffer d'E/S avant d'ecrire dessus :
	tcflush(fileDescriptor, TCIOFLUSH);
	int cr = write(fileDescriptor, data.c_str(), data.length());

	return cr;
} //----- Fin de envoyerDonnees
